# vPresent is an immersive presentation creation and display application.
# Copyright (C) 2007 by Infiscape Corporation
#
# This program is free software; you can redistribute it and/or modify it
# under the terms of the GNU General Public License as published by the Free
# Software Foundation; either version 2 of the License, or (at your option)
# any later version.
#
# This program is distributed in the hope that it will be useful, but WITHOUT
# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
# FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
# more details.
#
# You should have received a copy of the GNU General Public License along
# with this program; if not, write to the Free Software Foundation, Inc.,
# 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.

import math
import osg2 as osg


LEFT_MOUSE   = 0
MIDDLE_MOUSE = 1
RIGHT_MOUSE  = 2
UP_MOUSE     = 3
DOWN_MOUSE   = 4

IDLE               = 0
ROTATING           = 1
TRANSLATING_XY     = 2
TRANSLATING_Z      = 3
TRANSLATING_ZPLUS  = 10
TRANSLATING_ZMINUS = 11

class TrackballNavStrategy(object):
   '''
   A navigation strategy that is based on a virtual trackball. It is designed
   for use with nav.Navigator.
   
   This strategy supports continuous motion (i.e., throwing) by keeping track
   of changes between sequential move operations. The buttonRelease() method
   detects the case when continuous motion is to occur. The update() method
   processes the actual continuous motion using temporary state from the most
   recent move operation.

   @ivar mRadius:      The size of the virtual trackball on the screen.
   @ivar mTbMatrix:    The trackball matrix.
   @ivar mFinalMatrix: The transformation from the virtual world to the
                       virtual platform space (vw_M_vp).
   @ivar _from:        The center of the trackball (i.e., the location of the
                       transformation matrix).
   @ivar _at:          The location of the camera looking at the center of the
                       trackball.
   @ivar _moved:       Internal temporary that indicates whether the mouse has
                       moved since the last button press.
   @ivar _clickCenter: If True, a click on an object recenters the trackball
                       on the clicked (intersection) point.
   @ivar _isectPoint:  The point of intersection in the scene with a view
                       ray while panning. This is in virtual world coordinates.
   @ivar _dir:         Temporary view ray used for intersection point
                       calculation.
   '''

   def __init__(self, navigator, size = 0.8):
      self.mRadius      = size
      self.mTbMatrix    = osg.Matrix()
      self.mFinalMatrix = osg.Matrix()
      # NOTE: We may want to rename this to mCenter in the future. This may
      # be the center point of rotation for the trackball.
      self._from = osg.Pnt3f(0.0, 0.0, 0.0)
      self._at   = osg.Pnt3f(0.0, 0.0, 1.0)
      self._up   = osg.Vec3f(0.0, 1.0, 0.0)

      self._navigator       = navigator
      self._currentState    = IDLE
      self._motionFactor    = 1.0
      self._moved           = False
      self._clickCenter     = True
      self._viewport        = None
      self._lastX           = 0
      self._lastY           = 0
      self._lastXDelta      = 0
      self._lastYDelta      = 0
      self._isectPoint      = osg.Pnt3f(0.0, 0.0, 0.0)
      self._dir             = osg.Vec3f()
      self._continueMotion  = False
      self._lastRotationMat = None
      self._lastTransXYMat  = None
      self._lastDistance    = 0.0

      self.mDistance = (self._at - self._from).length()

#   up = property(getUp)

   def setNavigator(self, navigator):
      self._navigator = navigator

   def getMatrix(self):
      self._updateFinalMatrix()
      return self.mFinalMatrix

   def getFrom(self):
      trans_vec = self.mFinalMatrix[3]
      self._from.setValues(trans_vec[0], trans_vec[1], trans_vec[2])
      return self._from

   def getAt(self):
      trans_vec = self.mFinalMatrix[3]
      z_basis   = self.mFinalMatrix[2]
      #z_offset  = self.mDistance * z_basis
      z_offset  = osg.Vec4f(self.mDistance * z_basis[0],
                            self.mDistance * z_basis[1],
                            self.mDistance * z_basis[2],
                            self.mDistance * z_basis[3])
      at_4      = trans_vec - z_offset
      self._at.setValues(at_4[0], at_4[1], at_4[2])
      return self._at

   def getUp(self):
      y_basis = self.mFinalMatrix[1]
      self._up.setValues(y_basis[0], y_basis[1], y_basis[2])
      return self._up

   def getDistance(self):
      return self.mDistance

   def setFrom(self, newFrom):
      self.setView(newFrom, self.getAt(), self.getUp())

   def setAt(self, newAt):
      self.setView(self.getFrom(), newAt, self.getUp())

   def setDistance(self, newDistance):
      self.mDistance = newDistance
      self._updateFinalMatrix()

   def setUp(self, newUp):
      self.setView(self.getFrom(), self.getAt(), newUp)

   def setView(self, newFrom, newAt, newUp):
      self._moved = False

      # Ensure that we do not continue any current motion now that our
      # viewpoint has been changed externally.
      self._continueMotion = False

      view_at = newAt + (newAt - newFrom)
      failed = osg.MatrixLookAt(self.mTbMatrix, newAt, view_at, newUp)

      if not failed:
         self.mDistance = (newAt - newFrom).length()
         self._updateFinalMatrix()
      else:
         raise RuntimeError, "Invalid view matrix"

   def setMatrix(self, newMatrix):
      # Use mFinalMatrix as temporary storage. The final call to setView()
      # updates it with the "new" new matrix.
      self.mFinalMatrix.setValue(newMatrix)

      # Calling these get methods has the side effect of updating self._from,
      # self._at, and self._up respectively.
      self.setView(self.getFrom(), self.getAt(), self.getUp())

   def setMotionFactor(self, newFactor):
      self._motionFactor = newFactor
      
   def setViewport(self, viewport):
      self._viewport = viewport

   def buttonPress(self, button, x, y):
      self._moved = False

      # If we were perfoming continuous motion, then disable click-to-center
      # (see buttonRelease()) by indicating that a move happened.
      if self._continueMotion:
         self._moved = True

      self._continueMotion = False

      if LEFT_MOUSE == button:
         self._currentState = ROTATING
      elif RIGHT_MOUSE == button:
         self._currentState = TRANSLATING_Z
      elif MIDDLE_MOUSE == button:
         self._currentState = TRANSLATING_XY
         self._updateIntersectionPoint(x, y)
      elif UP_MOUSE == button:
         self._currentState = IDLE
         self.translateZ(-self._motionFactor)
      elif DOWN_MOUSE == button:
         self._currentState = IDLE
         self.translateZ(self._motionFactor)

      self._lastXDelta = self._lastYDelta = 0
      self._lastX, self._lastY = x, y

   def buttonRelease(self, button, x, y):
      eps = 2
      if self._lastXDelta > eps or self._lastYDelta > eps:
         self._continueMotion = True
      else:
         self._currentState = IDLE

   def buttonDoubleClick(self, button, x, y):
      if self._clickCenter:
         self._setNewCenter(x, y)

   def keyPress(self, key):
      pass

   def moveTo(self, x, y):
      def signOf(x):
         if x < 0:
            return -1
         if x > 0:
            return 1
         return 0

      self._moved = True
      from_x, from_y = self._viewport.getNormalizedCoordinates(self._lastX,
                                                               self._lastY)
      to_x, to_y = self._viewport.getNormalizedCoordinates(x, y)

      if ROTATING == self._currentState:
         self.rotate(from_x, from_y, to_x, to_y)
      elif TRANSLATING_XY == self._currentState:
         self.translateXY(x, y)
      elif TRANSLATING_Z == self._currentState:
         distance = signOf(to_y - from_y) * 100.0 * \
                    math.pow(math.fabs(to_y - from_y), 2.0)
         self.translateZ(distance * self._motionFactor)

      # Store the current delta for this move in the event that we need it in
      # update() for continued motion (i.e., "throwing").
      self._lastXDelta = math.fabs(self._lastX - x)
      self._lastYDelta = math.fabs(self._lastY - y)
      self._lastX, self._lastY = x, y

   def update(self, button, x, y):
      if self._continueMotion:
         if ROTATING == self._currentState:
            self.continueRotate()
         elif TRANSLATING_XY == self._currentState:
            self.continueTranslateXY()
         elif TRANSLATING_Z == self._currentState:
            self.translateZ(None)

   def rotate(self, fromX, fromY, toX, toY):
      '''
      Rotates the trackball about its center and updates the internal matrix.

      @note The X and Y values are in a normalized coordinate system where the
            lower left is (-1,-1) and the upper right is (1,1).
      '''
      def clamp(minVal, val, maxVal):
         if val > minVal:
            if val < maxVal:
               return val
            else:
               return maxVal
         else:
            return minVal

      eps = 1.0E-6
      if math.fabs(fromX - toX) > eps or math.fabs(fromY - toY) > eps:
         # Find the vectors from the center of the trackball to the
         # intersected points on the virtual sphere.
         from_vec = osg.Vec3f()
         from_vec.setValues(fromX, fromY,
                            self._projectToSphere(self.mRadius, fromX, fromY))
         to_vec = osg.Vec3f()
         to_vec.setValues(toX, toY,
                          self._projectToSphere(self.mRadius, toX, toY))

         axis = osg.Vec3f(to_vec)
         axis.crossThis(from_vec)

         diff  = osg.Vec3f(from_vec)
         diff -= to_vec

         tmp = clamp(-1.0, diff.length() / (2.0 * self.mRadius), 1.0)

         # The angle between the from point and the to point.
         phi = 2.0 * math.asin(tmp)

         # Create a quaternion that is the rotation about axis to get from
         # the from point to the to point.
         curr_val = osg.Quaternion()
         curr_val.setValueAsAxisRad(axis, phi)

         self._lastRotationMat = osg.Matrix()
         self._lastRotationMat.setRotate(curr_val)

         # Apply the rotation.
         self.mTbMatrix.mult(self._lastRotationMat)

   def continueRotate(self):
      self.mTbMatrix.mult(self._lastRotationMat)

   def translateXY(self, x, y):
      '''
      Translates in the XY plane.

      @param x: The X coordinate in the viewport.
      @param y: The Y coordinate in the viewport.
      '''
      distance_x, distance_y = self._calcDeltas(x, y)
      self._lastTransXYMat = osg.Matrix()
      self._lastTransXYMat.setIdentity()
      self._lastTransXYMat.setTranslate(distance_x, distance_y, 0)
      self.mTbMatrix.mult(self._lastTransXYMat)

   def continueTranslateXY(self):
      self.mTbMatrix.mult(self._lastTransXYMat)

   def translateZ(self, distance):
      '''
      Translates along the Z axis.

      @param distance: The distance to translate in the Z direction or None.
                       If None, the last distance translated is reused.
                       Otherwise, the value must be in world coordinates.
      '''
      if distance is None:
         self.mDistance += self._lastDistance
      else:
         self.mDistance     += distance
         self._lastDistance  = distance

   def _setNewCenter(self, x, y):
      '''
      Determines if a view ray at the given coordinates hits the scene, then
      re-center this trackball at that point of intersection.
      '''
      # Perform the intersection on the child of the virtual world if it has
      # one. This is the root of the virtual world.
      vw_node = self._navigator.getViewPlatformNode().getChild(0)

      if vw_node is not None:
         # The ray under the mouse in the virtual world coordinate frame
         # (vw_M_vp).
         line = self._navigator.calcVirtualWorldViewRay(x, y)

         isect_act = osg.IntersectAction.create()
         isect_act.setLine(line)
         isect_act.apply(vw_node)

         if isect_act.didHit():
            self.setAt(isect_act.getHitPoint())

   def _calcCamCoordToWorldCoord(self, vp_M_vw):
      '''
      Calculates the transformation matrix from CC to WC using the actual
      view matrix.
      '''
      proj_mat    = osg.Matrix()
      camera_M_vp = osg.Matrix()

      vp_width  = self._viewport.getPixelWidth()
      vp_height = self._viewport.getPixelHeight()

      camera = self._viewport.getCamera()
      # After this call, proj_mat is ndc_M_camera.
      camera.getProjection(proj_mat, vp_width, vp_height)
      camera.getProjectionTranslation(camera_M_vp, vp_width, vp_height)

      # ndc_M_vw = ndc_M_camera * camera_M_vp * vp_M_vw
      # (ndc = normalized coordinates)
      proj_mat.mult(camera_M_vp)
      proj_mat.mult(vp_M_vw)

      # Change from ndc_M_vw to vw_M_ndc.
      vw_M_ndc = osg.Matrix()
      vw_M_ndc.invertFrom(proj_mat)

      return vw_M_ndc

   def _updateIntersectionPoint(self, x, y):
      '''
      Calculates the intersection point of a ray that starts at from and goes
      through the position on the screen given by x,y with the world, if no
      intersection point exists the intersection is set to (0,0,0).
      '''
      vw_node = self._navigator.getViewPlatformNode().getChild(0)

      if vw_node is not None:
         # Get the ray under the mouse in virtual world coordinates.
         line = self._navigator.calcVirtualWorldViewRay(x, y)

         isect_act = osg.IntersectAction.create()
         isect_act.setLine(line)
         isect_act.apply(vw_node)

         width  = self._viewport.getPixelWidth()
         height = self._viewport.getPixelHeight()

         vp_M_vw = osg.Matrix()
         vp_M_vw.invertFrom(self.getMatrix())

#         vp_M_vw = osg.Matrix()
#         self._viewport.getCamera().getViewing(vp_M_vw, width, height)

         # XXX: We might want to send in an identity matrix for the view.
         vw_M_ndc = self._calcCamCoordToWorldCoord(vp_M_vw)

         at = osg.Pnt3f()
         to = osg.Pnt3f()
         vw_M_ndc.multFullMatrixPnt(osg.Pnt3f(0.0, 0.0, 0.5), to)
         vw_M_ndc.multFullMatrixPnt(osg.Pnt3f(0.0, 0.0, 1.0), at)

         # A vector pointing directly into the scene. This is in the
         # coordinate frame of the virtual world.
         self._dir = to - at

         # self._isectPoint is in virtual world coordinates.
         if isect_act.didHit():
            self._isectPoint = osg.Pnt3f(isect_act.getHitPoint())
         else:
            # What is this?
            u = self._dir.dot(osg.Pnt3f(0.0, 0.0, 0.0) - line.getPosition()) / \
                self._dir.dot(line.getDirection())
            self._isectPoint = line.getPosition() + line.getDirection() * u

   def _calcDeltas(self, toX, toY):
      '''
      Calculates the real translation that has to be done, so that the
      trackball can actually drag the object in the plane parallel to the
      screen.
      '''
      vw_M_vp = osg.Matrix(self.getMatrix())
      trans_vec = vw_M_vp[3]

      # The location where we as the viewer are sitting in the virtual world.
      from_pt = osg.Pnt3f(trans_vec[0], trans_vec[1], trans_vec[2])

      vp_M_vw = osg.Matrix()
      vp_M_vw.invertFrom(vw_M_vp)

      # Get the transformation from normalized device coordinates (ncd) to the
      # virtual world.
      vw_M_ndc = self._calcCamCoordToWorldCoord(vp_M_vw)

      x, y = self._viewport.getNormalizedCoordinates(toX, toY)

      # Transform the mouse position into virtual world coordinates (vw_M_vp).
      at = osg.Pnt3f()
      vw_M_ndc.multFullMatrixPnt(osg.Pnt3f(x, y, 1.0), at)
      vw_M_ndc.multFullMatrixPnt(osg.Pnt3f(x, y, 0.5), from_pt)

      # Define the mouse ray in virtual world coordinates. The ray points
      # out of the scene.
      line = osg.Line()
      line.setValue(from_pt, at - from_pt)

      # The delta.
      u = self._dir.dot(self._isectPoint - line.getPosition()) / \
          self._dir.dot(line.getDirection())

      # The point on the virtual plane with which we are intersecting.
      p2 = line.getPosition() + line.getDirection() * u
      transl = osg.Vec3f(-p2[0] + self._isectPoint[0],
                         -p2[1] + self._isectPoint[1],
                         -p2[2] + self._isectPoint[2])

      vp_M_vw.multMatrixVec(transl)

      return transl[0], transl[1]

   def _projectToSphere(self, radius, x, y):
      '''
      Projects a point on the virtual trackball. If it is inside the sphere,
      map it to the sphere. If it is outside, map it to a hyperbola.

      @note x and y are in a normalized coordinate system where the lower left
            is (-1,-1) and the upper right is (1,1). radius is the radius of
            the virtual sphere with a valid range of [0.0,1.0].
      '''
      # The distance from the center.
      d = math.sqrt(x * x + y * y)

      # Inside the sphere.
      # NOTE: This magic number (sqrt(2) / 2) is necessary to prevent jitter
      # from occuring at the edge of the virtual trackball sphere when
      # rotating.
      if d < radius * 0.70710678118654752440:
         z = math.sqrt(radius * radius - d * d)
      # On hyperbola.
      else:
         t = radius / 1.41421356237309504880
         z = t * t / d

      return z

   def _updateFinalMatrix(self):
      '''
      Updates the final matrix by setting it to the current trackball matrix
      with a post-translate from the center of the trackball.
      '''
      self.mFinalMatrix.setValue(self.mTbMatrix)

      temp = osg.Matrix()
      temp.setIdentity()
      temp.setTranslate(0.0, 0.0, self.mDistance)

      self.mFinalMatrix.mult(temp)

class Navigator(object):
   '''
   This is the main controller for all navigation in the application. It uses
   pluggable navigation strategies to do the real navigation work. To this
   class, the steps are as follows:

      1. Create an instance
      2. Set the viewport within which interaction occurs
      3. Set the cart node for the camera
      4. Set a navigation strategy
      5. Call the event processing methods accordingly
      6. Update the camera transformation

   The coordinate spaces used by this class are the following:

      * VP: The view platform that is moving through the scene. Because we
            are in desktop mode, it is also the camera space.
      * VW: The virtual world/scene space. This is the actual environment
            through which we are navigating.
   '''

   def __init__(self):
      self._viewPlatformNode = None
      self.mNavStrategy      = TrackballNavStrategy(self)

      self.setMotionFactor(1.0)
      self.setViewport(None)

   def setStrategy(self, navStrategy):
      self.mNavStrategy = navStrategy
      self.mNavStrategy.setNavigator(self)
      self.mNavStrategy.setViewport(self._viewport)
      self.mNavStrategy.setMotionFactor(self._motionFactor)

   def setMotionFactor(self, newFactor):
      self._motionFactor = newFactor
      self.mNavStrategy.setMotionFactor(newFactor)

   def setViewport(self, viewport):
      self._viewport = viewport
      self.mNavStrategy.setViewport(viewport)

   def setViewPlatformNode(self, newVpNode):
      self._viewPlatformNode = newVpNode

   def getViewPlatformNode(self):
      return self._viewPlatformNode

   def getMatrix(self):
      return self.mNavStrategy.getMatrix()

   def setMatrix(self, matrix):
      self.mNavStrategy.setMatrix(matrix)

   def reset(self):
      self.setMatrix(osg.Matrix())

   def buttonPress(self, button, x, y):
      '''
      @note x and y are in viewport coordinates. The upper left corner is (0,0).
      '''
      self.mNavStrategy.buttonPress(button, x, y)

   def buttonRelease(self, button, x, y):
      '''
      @note x and y are in viewport coordinates. The upper left corner is (0,0).
      '''
      self.mNavStrategy.buttonRelease(button, x, y)

   def buttonDoubleClick(self, button, x, y):
      '''
      @note x and y are in viewport coordinates. The upper left corner is (0,0).
      '''
      self.mNavStrategy.buttonDoubleClick(button, x, y)

   def keyPress(self, key):
      '''
      @note x and y are in viewport coordinates. The upper left corner is (0,0).
      '''
      self.mNavStrategy.keyPress(key)

   def moveTo(self, x, y):
      '''
      @note x and y are in viewport coordinates. The upper left corner is (0,0).
      '''
      self.mNavStrategy.moveTo(x, y)

   def update(self, button, x, y):
      '''
      @note x and y are in viewport coordinates. The upper left corner is (0,0).
      '''
      self.mNavStrategy.update(button, x, y)

      vp_matrix = osg.Matrix(self.mNavStrategy.getMatrix())
      vp_matrix.invert()

      xform_core = self._viewPlatformNode.getCore()
      xform_core.setMatrix(vp_matrix)

   def calcVirtualWorldViewRay(self, x, y):
      '''
      Returns the view ray in the virtual world coordinate space (i.e.,
      vw_M_vp).
      '''
      # The line in the *view platform* space.
      line = self._viewport.getCamera().calcViewRay(x, y, self._viewport)[1]

      pos = osg.Pnt3f()
      dir = osg.Vec3f()

      # Transform the ray into the coordinate frame of the virtual world from
      # the view platform. This has to be done manually because OpenSG does
      # not have a built-in mechanism for transforming a Line by a Matrix.
      vw_M_vp = self.mNavStrategy.getMatrix()
      vw_M_vp.multFullMatrixPnt(line.getPosition(), pos)
      vw_M_vp.multMatrixVec(line.getDirection(), dir)

      return osg.Line(pos, dir)
